import java.io.DataOutputStream;
import java.util.ArrayList;


import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.Sound;

//enum Protocol {PING, STUP, STSP, STOP, STFU, SCAN, TRCK, ANGL, PASE};
//enum Motors {A, B, C};

public class NXTCommands {

	//private DataOutputStream _outputStream;
	private ClientManager _clientManager;
	private Motor _motorRight, _motorLeft, _motorSensor;
	private boolean isOn = false;
	
	public NXTCommands(ClientManager client){
		_clientManager = client;
		//_outputStream = out;
		_motorRight = Motor.A;
		_motorLeft = Motor.B;
		_motorSensor = Motor.C;
	}
	
	private void log(String str, int a, int b) {
		LCD.drawString("'" + str + "'", a, b);
		LCD.refresh();
	}
	
	public void receivedMessage(String msg)
	{
		if(msg.length() < 4) {Sound.buzz(); return;}
		String head = msg.substring(0, 4);
		String body = msg.length() > 5 ? msg.substring(5, msg.length()) : "";

		//log(head, 2, 2);
		if(head.equals("STSP")) setSpeed(body);
		if(head.equals("STOP")) stopMotors();
		if(head.equals("STAR")) start();
		if(head.equals("SCAN")) ultraSonicSensorOn(body);
		if(head.equals("SCSP")) setScanSpeed(body);
	}
	
	private void start() {
		isOn = true;
	}
	
	private Motor motorFromString(String str) {
		if(str.equals("A")) return Motor.A;
		if(str.equals("B")) return Motor.B;
		if(str.equals("C")) return Motor.C;
		return null;
	}
	
	private final float _motorSpeedMultiplier = 720.0f;
	private void setSpeed(Motor motor, float speed) {
		if(speed > 0) { motor.setSpeed((int)(speed * _motorSpeedMultiplier)); motor.forward(); }
		else { motor.setSpeed(-(int)(speed * _motorSpeedMultiplier)); motor.backward(); }
	}	
	
	public void setSpeed(String body) {
		if(!isOn) return;
		ArgumentEnumerator en = new ArgumentEnumerator(body, " ");
		String motor = en.getNext();
		float speed = 0.0f;
		try {
			speed = Float.parseFloat(en.getNext());
		} catch (NumberFormatException e) {
			// Handle
		}
		setSpeed(motorFromString(motor), speed);
	}
	
	private void ultraSonicSensorOn(String args) {
		boolean isOn = args.equals("0") ? false : true;
		_clientManager.sensorTimerTask.turnSensorOn(isOn);
	}
	
	private void setScanSpeed(String body) {
		int speed = Integer.parseInt(body);
		_clientManager.sensorTimerTask.setSensorSpeed(speed);
	}
	
	public void stopMotors() {
		isOn = false;
		_motorLeft.stop();
		_motorRight.stop();
		//Motor.C.stop();
	}
}
